#ifndef __DALELIB_QUAT_H__
#define __DALELIB_QUAT_H__

#include <dalelib/maths/v3.h>
#include <dalelib/maths/v4.h>
#include <dalelib/maths/m4.h>


namespace dalelib {

#define QUAT_EPSILON 0.00001f

class quat
{
private:

	float f[4]; // w + xi + yj + zk

public:

	quat() { }
	quat(const float _Value0, const float _Value1, const float _Value2, const float _Value3)
	{
		f[0] = _Value0; f[1] = _Value1; f[2] = _Value2; f[3] = _Value3;
	}
	quat(float _Scalar,const v3 _Vec) //  be careful not to mix with following constructor
	{
		f[0] = _Scalar; f[1] = _Vec[0]; f[2] = _Vec[1]; f[3] = _Vec[2];
	}
	quat(const v3 _Vec, float _Ang)
	{
		_Ang *= 0.5f;
		float result = sinf( _Ang );
		f[0] = cosf( _Ang ); f[1] = _Vec[0]*result; f[2] = _Vec[1]*result; f[3] = _Vec[2]*result;
	}
	static const quat zero;
	static const quat identity;

	const float& w() const { return f[0]; }
	const float& x() const { return f[1]; }
	const float& y() const { return f[2]; }
	const float& z() const { return f[3]; }
	v3 getVecPart() { return v3(f[1],f[2],f[3]); }
	const float& operator[](const int _Index) const { return f[_Index]; }

	quat operator-() const { return quat(-f[0],-f[1],-f[2],f[3]); }

	void operator+=(const float _Float) { f[0] += _Float; f[1] += _Float; f[2] += _Float; f[3] += _Float; }
	void operator-=(const float _Float) { f[0] -= _Float; f[1] -= _Float; f[2] -= _Float; f[3] -= _Float; }
	void operator*=(const float _Float) { f[0] *= _Float; f[1] *= _Float; f[2] *= _Float; f[3] *= _Float; }
	void operator/=(const float _Float) { f[0] /= _Float; f[1] /= _Float; f[2] /= _Float; f[3] /= _Float; }

	quat operator+(const float _Float) const { return quat( f[0]+_Float, f[1]+_Float, f[2]+_Float, f[3]+_Float ); }
	quat operator-(const float _Float) const { return quat( f[0]-_Float, f[1]-_Float, f[2]-_Float, f[3]-_Float ); }
	quat operator*(const float _Float) const { return quat( f[0]*_Float, f[1]*_Float, f[2]*_Float, f[3]*_Float ); }
	quat operator/(const float _Float) const { return quat( f[0]/_Float, f[1]/_Float, f[2]/_Float, f[3]/_Float ); }

	void operator+=(const quat _Quat) { f[0] += _Quat.f[0]; f[1] += _Quat.f[1]; f[2] += _Quat.f[2]; f[3] += _Quat.f[3]; }
//	void operator-=(const quat _Value) { f[0] -= _Value[0]; f[1] -= _Value[1]; f[2] -= _Value[2]; }
//	void operator*=(const quat _Value) { f[0] *= _Value[0]; f[1] *= _Value[1]; f[2] *= _Value[2]; }
//	void operator/=(const quat _Value) { f[0] /= _Value[0]; f[1] /= _Value[1]; f[2] /= _Value[2]; }

//	quat operator+(const quat _Value) const { return quat( f[0]+_Value[0], f[1]+_Value[1], f[2]+_Value[2] ); }
//	quat operator-(const quat _Value) const { return quat( f[0]-_Value[0], f[1]-_Value[1], f[2]-_Value[2] ); }
	quat operator *(const quat &_Quat)
	{
		return quat(	f[0]*_Quat[0] - f[1]*_Quat[1] - f[2]*_Quat[2] - f[3]*_Quat[3],
						f[0]*_Quat[1] + f[1]*_Quat[0] + f[2]*_Quat[3] - f[3]*_Quat[2],
						f[0]*_Quat[2] - f[1]*_Quat[3] + f[2]*_Quat[0] + f[3]*_Quat[1],
						f[0]*_Quat[3] + f[1]*_Quat[2] - f[2]*_Quat[1] + f[3]*_Quat[0] );
	}

	float len2() const { return dot(*this); }
	float len() const { float l2=len2(); return (l2>QUAT_EPSILON)?sqrtf(l2):0.f;  }

	quat unit() const { float l=len(); return (l>QUAT_EPSILON)?(*this/l):quat::identity; }
	quat conjugate() const { return quat(f[0],-f[1],-f[2],-f[3]); } // conjugate is inverse for unit quaternions

	void normalise() { float l=len(); if(l>QUAT_EPSILON) *this/=l; else *this=quat::identity; }

	float dot(const quat _Quat) const { return f[0]*_Quat.f[0] + f[1]*_Quat.f[1] + f[2]*_Quat.f[2] + f[3]*_Quat.f[3]; }

	//quat cross(const quat _Value) const { return quat( f[1]*_Value[2]-f[2]*_Value[1], f[2]*_Value[0]-f[0]*_Value[2], f[0]*_Value[1]-f[1]*_Value[0] ); }
//	quat cross(const quat _Value) const {
//		return quat(
//			f[1]*_Value[2] - f[2]*_Value[1],
//			f[2]*_Value[0] - f[0]*_Value[2],
//			f[0]*_Value[1] - f[1]*_Value[0]
//		); }

//	quat saturate() const { return quat( SATURATE(f[0]), SATURATE(f[1]), SATURATE(f[2]) ); }

	m4 toMatrix()
	{
		const float TWOX2 = 2*f[1]*f[1], TWOY2 = 2*f[2]*f[2], TWOZ2 = 2*f[3]*f[3];
		const float TWOWX = 2*f[0]*f[1], TWOWY = 2*f[0]*f[2], TWOWZ = 2*f[0]*f[3];
		const float TWOXY = 2*f[1]*f[2], TWOXZ = 2*f[1]*f[3], TWOYZ = 2*f[2]*f[3];

		return m4(	1.f-TWOY2-TWOZ2, TWOXY+TWOWZ,     TWOXZ-TWOWY,     0.f,
						TWOXY-TWOWZ,     1.f-TWOX2-TWOZ2, TWOYZ+TWOWX,     0.f,
						TWOXZ+TWOWY,     TWOYZ-TWOWX,     1.f-TWOX2-TWOY2, 0.f,
						0.f,             0.f,             0.f,             1.f );
	}

	void fromMatrix(m4 &_Mat)
	{
		static const float M_EPSILON = 0.00001f;

		m4 m;

		float t = _Mat[0]+_Mat[5]+_Mat[10]+1.f;

		if( t>M_EPSILON )
		{
			float s = 0.5f/sqrtf(t);
			f[0] = 0.25f/s;
			f[1] = (_Mat[6]-_Mat[9])*s;
			f[2] = (_Mat[8]-_Mat[2])*s;
			f[3] = (_Mat[1]-_Mat[4])*s;
		}
		else
		{
			if( _Mat[0]>_Mat[5] && _Mat[0]>_Mat[10] )
			{
				float s = 2.f*sqrtf(1.f+_Mat[0]-_Mat[5]-_Mat[10]);
				f[0] = (_Mat[6]-_Mat[9])/s;
				f[1] = 0.25f*s;
				f[2] = (_Mat[4]+_Mat[1])/s;
				f[3] = (_Mat[8]+_Mat[2])/s;
			}
			else if(_Mat[5]>_Mat[10])
			{
				float s = 2.f*sqrtf(1.f+_Mat[5]-_Mat[0]-_Mat[10]);
				f[0] = (_Mat[8]-_Mat[2])/s;
				f[1] = (_Mat[4]+_Mat[1])/s;
				f[2] = 0.25f*s;
				f[3] = (_Mat[9]+_Mat[6])/s;
			}
			else
			{
				float s = 2.f*sqrtf(1.f+_Mat[10]-_Mat[0]-_Mat[5]);
				f[0] = (_Mat[1]-_Mat[4])/s;
				f[1] = (_Mat[8]+_Mat[2])/s;
				f[2] = (_Mat[9]+_Mat[6])/s;
				f[3] = 0.25f*s;
			}
		}
		normalise();
	}

	void print(const string _String) { printf( "%s %.3f %.3f %.3f %.3f\n", _String.c_str(), f[0], f[1], f[2], f[3] ); }

};

#undef QUAT_EPSILON

} // namespace dalelib

#endif
